#include <stdlib.h>
#include <stdint.h>
#include <stdbool.h>
#include <ctype.h>
#include <string.h>

#include "demo_system.h"
#include "dev_access.h"
#include "gpio.h"
#include "uart.h"


// format conversion

int  xtoi( char x ) {
  if     ( ( x >= '0' ) && ( x <= '9' ) ) {
    return ( ( int )( x ) - ( int )( '0' )      );
  }
  else if( ( x >= 'a' ) && ( x <= 'f' ) ) {
    return ( ( int )( x ) - ( int )( 'a' ) + 10 );
  }
  else if( ( x >= 'A' ) && ( x <= 'F' ) ) {
    return ( ( int )( x ) - ( int )( 'A' ) + 10 );
  }

  return -1;
}

char itox( int  x ) {
  if     ( ( x >=  0 ) && ( x <=  9 ) ) {
    return ( char )( ( int )( '0' ) + x       );
  }
  else if( ( x >= 10 ) && ( x <= 15 ) ) {
    return ( char )( ( int )( 'A' ) + x - 10  );
  }

  return '?';
}

void xtob( uint8_t* r, char* x, int n ) {
  for( int i = 0; i < n; i++ ) {
    r[ i ]  = ( uint8_t )( ( xtoi( *x++ ) & 0xF ) << 4 );
    r[ i ] |= ( uint8_t )( ( xtoi( *x++ ) & 0xF ) << 0 );
  }
}

void btox( char* r, uint8_t* x, int n ) {
  for( int i = 0; i < n; i++ ) {
    *r++ = itox( ( uint8_t )( ( x[ i ] >> 4 ) & 0xF ) );
    *r++ = itox( ( uint8_t )( ( x[ i ] >> 0 ) & 0xF ) );
  }

  *r++ = '\x00';
}


// I/O via UART: raw

void    uart_wr  ( uart_t uart, uint8_t x ) {
  while( DEV_READ( uart + UART_STATUS_REG ) & UART_STATUS_TX_FULL  ) {
    /* spin */
  }

         DEV_WRITE( uart + UART_TX_REG, x );
}

uint8_t uart_rd  ( uart_t uart            ) {
  while( DEV_READ( uart + UART_STATUS_REG ) & UART_STATUS_RX_EMPTY ) { 
    /* spin */
  }

  return DEV_READ ( uart + UART_RX_REG    );
}


// I/O via UART: structured

void octetstr_wr( uart_t uart, const uint8_t* x, int n_x ) {
             uart_wr( uart, itox( ( n_x      >> 4 ) & 0xF ) );
             uart_wr( uart, itox( ( n_x      >> 0 ) & 0xF ) );

             uart_wr( uart, ':'    );

  for( int i = 0; i < n_x; i++ ) {
             uart_wr( uart, itox( (   x[ i ] >> 4 ) & 0xF ) );
             uart_wr( uart, itox( (   x[ i ] >> 0 ) & 0xF ) );
  }

             uart_wr( uart, '\x0D' );
}

int  octetstr_rd( uart_t uart,       uint8_t* r, int n_r ) {
  int n  = ( ( xtoi( uart_rd( uart ) ) & 0xF ) << 4 );
  
      n |= ( ( xtoi( uart_rd( uart ) ) & 0xF ) << 0 );

  if( n > n_r ) {
    return 0xF1000000 |              ( n << 0 );
  }

  uint8_t t;

  if( ( t = uart_rd( uart ) ) != ':' ) {
    return 0xF2000000 | ( t << 8 ) | ( n << 0 );
  }

  int i = 0;

  for( i = 0; i < n; i++ ) {
    r[ i ] = 0;

    if( ( ( t = uart_rd( uart ) ) == '\x0D' ) || !isxdigit( t ) ) {
      break;
    }
    r[ i ] |= ( uint8_t )( xtoi( t ) & 0xF ) << 4;

    if( ( ( t = uart_rd( uart ) ) == '\x0D' ) || !isxdigit( t ) ) {
      break;
    }
    r[ i ] |= ( uint8_t )( xtoi( t ) & 0xF ) << 0;
  }

  while( t != '\x0D' ) {
    t = uart_rd( uart );
  }

  return i;
}

void ack_wr( uart_t uart, bool x ) {
  if( x ) {
    uart_wr( uart, '+' ); uart_wr( uart, '\x0D' );
  }
  else {
    uart_wr( uart, '-' ); uart_wr( uart, '\x0D' );
  }
}

bool ack_rd( uart_t uart         ) {
  uint8_t x = uart_rd( uart );
  uint8_t t = uart_rd( uart );

  while( t != '\x0D' ) {
    t = uart_rd( uart );
  }

  return ( x == '+' ) ? ( true ) : ( false );
}


// main function 

int main( int argc, char* argv[] ) {
  set_global_interrupt_enable( 0 );

  while(1){
    uint8_t temp[4];
    uart_t uart = DEFAULT_UART;

    //input x0
    uint32_t x0;
    octetstr_rd( uart, temp, 4 );
    x0  = (uint32_t) temp[0] << 24;
    x0 |= (uint32_t) temp[1] << 16;
    x0 |= (uint32_t) temp[2] <<  8;
    x0 |= (uint32_t) temp[3]      ;
    // puts("The x0 is:\n");
    // puthex(x0);
    // puts("\n");

    //input x1
    uint32_t x1;
    octetstr_rd( uart, temp, 4 );
    x1  = (uint32_t) temp[0] << 24;
    x1 |= (uint32_t) temp[1] << 16;
    x1 |= (uint32_t) temp[2] <<  8;
    x1 |= (uint32_t) temp[3]      ;
    // puts("The x1 is:\n");
    // puthex(x1);
    // puts("\n");

    //input y0
    uint32_t y0;
    octetstr_rd( uart, temp, 4 );
    y0  = (uint32_t) temp[0] << 24;
    y0 |= (uint32_t) temp[1] << 16;
    y0 |= (uint32_t) temp[2] <<  8;
    y0 |= (uint32_t) temp[3]      ;
    // puts("The y0 is:\n");
    // puthex(y0);
    // puts("\n");

    //input y1
    uint32_t y1;
    octetstr_rd( uart, temp, 4 );
    y1  = (uint32_t) temp[0] << 24;
    y1 |= (uint32_t) temp[1] << 16;
    y1 |= (uint32_t) temp[2] <<  8;
    y1 |= (uint32_t) temp[3]      ;
    // puts("The y1 is:\n");
    // puthex(y1);
    // puts("\n");

    uint32_t back[8];
    uint32_t res1;
    uint32_t res0;

    //x0,x1,y0,y1 cross P and in t3,t4,t5,t6
    asm volatile(
      ".include \"ise.S\"\n\t"
      "sw        t3, %[t3b]\n\t"
      "lw        t3, %[srcx0]\n\t"
      "cbm.or    t3, t3, x0, 0\n\t"
      "cbm.or    t3, t3, x0, 3\n\t"
      "sw        t4, %[t4b]\n\t"
      "lw        t4, %[srcx1]\n\t"
      "cbm.or    t4, t4, x0, 0\n\t"
      "cbm.or    t4, t4, x0, 3\n\t"
      "sw        s9, %[s9b]\n\t"
      "li        s9, 0\n\t"
      "nop\n\t"
      "nop\n\t"
      "nop\n\t"
      "nop\n\t"
      "nop\n\t"
      "nop\n\t"
      "nop\n\t"
      "nop\n\t"
      "nop\n\t"
      "nop\n\t"
      "nop\n\t"
      "nop\n\t"
      "cbm.or     s9, x0, x0, 3\n\t"
      "nop\n\t"
      "nop\n\t"
      "nop\n\t"
      "nop\n\t"
      "nop\n\t"
      "nop\n\t"
      "nop\n\t"
      "nop\n\t"
      "nop\n\t"
      "nop\n\t"
      "nop\n\t"
      "nop\n\t"
      "cbm.srli   s9, s9, 16, 3\n\t"    //r
      "sw         t5, %[t5b]\n\t"
      "lw         t5, %[srcy0]\n\t"
      "cbm.or     t5, t5, x0, 0\n\t"
      "cbm.or     t5, t5, x0, 3\n\t"
      "sw         s11,%[s11b]\n\t"
      "sw         s10,%[s10b]\n\t"
      "sw         s8, %[s8b]\n\t"
      "sw         t6, %[t6b]\n\t"
      "lw         t6, %[srcy1]\n\t"
      "cbm.or     t6, t6, x0, 0\n\t"
      "cbm.or     t6, t6, x0, 3\n\t"
      "nop\n\t"
      "nop\n\t"
      "nop\n\t"
      "nop\n\t"
      "nop\n\t"
      :[t3b]"=m"(back[0]),[t4b]"=m"(back[1]),[t5b]"=m"(back[2]),[t6b]"=m"(back[3]),[s9b]"=m"(back[4]),[s10b]"=m"(back[5]),[s11b]"=m"(back[6]),[s8b]"=m"(back[7])
      :[srcx0]"m"(x0),[srcx1]"m"(x1),[srcy0]"m"(y0),[srcy1]"m"(y1)
      :"t3","t4","t5","t6","x0","x1","s8","s9","s10","s11"
    );

    //gpio up
    // set_outputs( GPIO_OUT, 0x1 );
    DEV_WRITE(GPIO_OUT, 0x1);

    //t3    x0
    //t4    x1
    //t5    y0
    //t6    y1
    asm volatile(
      "nop\n\t"
      "nop\n\t"
      "nop\n\t"
      "nop\n\t"
      "nop\n\t"
      "nop\n\t"
      "nop\n\t"
      "nop\n\t"
      "cbm.and    s10, t3,  t5,  3\n\t"    // x0y0
      "xor        s10, s10, s9\n\t"    // x0y0 ^ r
      "cbm.and    s11, t3,  t6,  3\n\t"    // x0y1
      "xor        s11, s11, s9\n\t"    // x0y1 ^ r
      "cbm.and    s8,  t4,  t5,  3\n\t"    // x1y0
      "xor        s11, s11, s8\n\t"    // x0y1 ^ r ^ x1y0
      "cbm.and    s8,  t4,  t6,  3\n\t"    // x1y1
      "xor        s11, s11, s8\n\t"    // x0y1 ^ r ^ x1y0 ^ x1y1
      "nop\n\t"
      "nop\n\t"
      "nop\n\t"
      "nop\n\t"
      "nop\n\t"
      "nop\n\t"
      "nop\n\t"
      "nop\n\t"
      "nop\n\t"
      "nop\n\t"
      "nop\n\t"
      "nop\n\t"
      "nop\n\t"
      "nop\n\t"
      "nop\n\t"
      "nop\n\t"
    );
    
    //gpio down
    // set_outputs( GPIO_OUT, 0x0 );
    DEV_WRITE(GPIO_OUT, 0x0);

    //sw and reset
    asm volatile(
      "sw        s10, %[res0]\n\t"
      "sw        s11, %[res1]\n\t"
      "lw        s8,  %[s8b]\n\t"
      "lw        s9,  %[s9b]\n\t"
      "lw        s10, %[s10b]\n\t"
      "lw        s11, %[s11b]\n\t"
      "lw        t3,  %[t3b]\n\t"
      "lw        t4,  %[t4b]\n\t"
      "lw        t5,  %[t5b]\n\t"
      "lw        t6,  %[t6b]\n\t"
      :[res1]"=m"(res1),[res0]"=m"(res0)
      :[t3b]"m"(back[0]),[t4b]"m"(back[1]),[t5b]"m"(back[2]),[t6b]"m"(back[3]),[s9b]"m"(back[4]),[s10b]"m"(back[5]),[s11b]"m"(back[6]),[s8b]"m"(back[7])
    );

    // puts("The res1 is:\n");
    // puthex(res1);
    // puts("\n");
    // puts("The res2 is:\n");
    // puthex(res2);
    // puts("\n");
    // puts("ok\n");
  }
  return 0;
}
